#ifndef CAMERA_H
#define CAMERA_H
#ifndef M_PI
#define M_PI 3.1415926535897932384626433832795
#endif
#include "vecmat.h"

class Camera {
private:
	mat<float,3,3> getRotationMatrix(float theta, float x, float y, float z)
	{
		theta *= (float) M_PI/180;  // put theta in radians
		vec<float, 3> u;
		u[0] = x;
		u[1] = y;
		u[2] = z;
		u = u / u.l2();
		x = u[0];
		y = u[1];
		z = u[2];
		float s = sin(theta);
		float c = cos(theta);
		/*
		*         Row Major Matrix for Rotation
		*
		*  float rotation[] = { x^2(1-c)+c     xy(1-c)-zs     xz(1-c)+ys     0
		*  					    yx(1-c)+zs     y^2(1-c)+c     yz(1-c)-xs     0
		*  					    xz(1-c)-ys     yz(1-c)+xs     z^2(1-c)+c     0
		*						    0               0              0         1
		*/
		float rotation[] = { x*x*(1-c)+c,   y*x*(1-c)+z*s, x*z*(1-c)-y*s,
							 x*y*(1-c)-z*s, y*y*(1-c)+c,   y*z*(1-c)+x*s,
							 x*z*(1-c)+y*s, y*z*(1-c)-x*s, z*z*(1-c)+c   };
		return mat<float,3,3>(rotation);
		//return Matrix(rotation, false);
	}


public:
	vec<float,3> loc;
	vec<float,3> face;
	vec<float,3> up;

	Camera()
	{
		loc[0] = 0;
		loc[1] = 0;
		loc[2] = 0;

		face[0] =  0;
		face[1] =  0;
		face[2] = -1;

		up[0] = 0;
		up[1] = 1;
		up[2] = 0;
	}

    /*
	// moves camera in global coordinates
 	void translateWorldSpace(float x, float y, float z)
	{
		loc[0] += x;
		loc[1] += y;
		loc[2] += z;
	}
    */

    vec<float,3> getTranslatedLocation(float right, float up, float back)
    {
        vec<float,3> newPos = loc;
        newPos += face.cross(this->up) * right +
                  this->up * up +
                  (face*-1) * back;
		newPos[1] = 0;
        return newPos;
    }

	// moves camera relative to it's current orientation
	void translateCameraSpace(float right, float up, float back)
	{
		loc = getTranslatedLocation(right, up, back);

        // printf("loc:\t(%f, %f, %f)\n", loc[0],loc[1],loc[2]);
	}

    /*
	// rotates camera, without moving it; axis given in global coordinates
	void rotateWorldSpace(float theta, float x, float y, float z)
	{
		mat<float,3,3> rotate = getRotationMatrix(theta, x, y, z);
		face = rotate * face;
		up   = rotate * this->up;
	}
    */

	// rotates camera, without moving it; axis in camera space coordinates
	void rotateCameraSpace(float theta, float right, float up, float back)
	{
		if (right == 0 && up == 0 && back == 0)
			return;
		vec<float, 3> orientation = 
			face.cross(this->up) * right +
			this->up * up +
			(face * -1) * back;
		mat<float,3,3> rotate = getRotationMatrix(theta, orientation[0], orientation[1], orientation[2]);
		vec<float,3> temp = rotate * face;
		if (abs(temp[1]) < 0.9)
		{
            face = temp.normalize();
			this->up = rotate * this->up;
			correctUp();
            this->up = this->up.normalize();
		}

        //printf("face: (%f, %f, %f)   |   up: (%f, %f, %f)\n",
        //    face[0],face[1],face[2],
        //    this->up[0],this->up[1],this->up[2]);
	}

	void correctUp() 
{
		up = 0,1,0;
		vec<float,3> r = face.cross(up);
		up = r.cross(face);
	}

	void use() const
	{
		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		// set the matrix cooresponding to the camera
		gluLookAt(loc[0], loc[1], loc[2],  
				  loc[0]+face[0], loc[1]+face[1], loc[2]+face[2], 
				  up[0], up[1], up[2]);
	}
};

#endif